from ctypes import *

import numpy as np
import numpy.ctypeslib as npct

# lib = cdll.LoadLibrary("/home/st/catkin_ws/devel/lib/libndt_python.so")  # 注意是相对于执行python命令时的路径
lib = cdll.LoadLibrary("/home/st/ubuntu_data/test_data/test8_ws/devel/lib/libicp_python.so")  # 注意是相对于执行python命令时的路径
# lib = npct.load_library("libndt_python.so", "/home/st/ubuntu_data/test_data/test8_ws/devel/lib/")  # 注意是相对于执行python命令时的路径

def icp_python(pc_crop, pc_car, pose_int):
    pose_output = np.zeros(shape=(7,), dtype=np.float32)
    lib.icp_python.argtypes = [
        npct.ndpointer(dtype=np.float32, ndim=2, shape = (pc_crop.shape[0], 4)),
        c_int,
        npct.ndpointer(dtype=np.float32, ndim=2, shape = (pc_car.shape[0], 4)),
        c_int,
        npct.ndpointer(dtype=np.float32, ndim=1),
        npct.ndpointer(dtype=np.float32, ndim=1)
    ]
    lib.icp_python(pc_crop, pc_crop.shape[0], pc_car, pc_car.shape[0], pose_int, pose_output)
    return pose_output

# print(lib)
# pc_crop = np.fromfile("/home/st/ubuntu_data/dataset/gazebo/0509/car/velodyne/000000.bin", dtype=np.float32).reshape(-1, 4)
pc_crop = np.fromfile("/home/st/ubuntu_data/dataset/gazebo/0509/training/velodyne/000000.bin", dtype=np.float32).reshape(-1, 4)
pc_car = np.fromfile("/home/st/ubuntu_data/dataset/gazebo/0509/car/000000.bin", dtype=np.float32).reshape(-1, 4)

# mask = (pc_crop[:,0]<20) & (pc_crop[:,0] >= -20) & (pc_crop[:,1] < 20) & (pc_crop[:,1] >= -20) & (pc_crop[:,2] <1.5) & (pc_crop[:,2]>=-0.72)
mask = (pc_crop[:,0]<20) & (pc_crop[:,0] >= -20) & (pc_crop[:,1] < 0.5) & (pc_crop[:,1] >= -0.4) & (pc_crop[:,2] <1.5) & (pc_crop[:,2]>=-0.72)
pc_crop = pc_crop[mask]

# pc_crop_xyz = pc_crop[:,0:4]   #if [:, 0:3] radom bug
# pc_car_xyz = pc_car[:,0:4]
# pc_car_xyz.tofile("/home/st/ubuntu_data/dataset/gazebo/0509/car/car_3_channel.bin")
pose_int = np.array([-3.9,0.0,0.7,1,0,0,0],dtype=np.float32)
# print (icp_python(pc_crop, pc_car, pose_int))  # [ 3. 20.  8.]
print (icp_python(pc_car, pc_crop, pose_int))  # [ 3. 20.  8.]
